/*
 * Copyright (C) 2017-2019, AEye, Inc.
 * All rights reserved.
 */
#include <iostream>
#include <memory>
#include <sstream>

#include "Sensor.hpp"
#include "AEyeSensorAPI.h"
#include "AEyeSDKEnumsToStr.hpp"
#include "SensorReader.hpp"
#include "SensorCommander.hpp"
#include "PcapPacket.hpp"
#include "ecast.h"

#ifdef _MSC_VER
#include <stdio.h>
#include <stdlib.h>
#include <iphlpapi.h>
#define MALLOC(x) HeapAlloc(GetProcessHeap(), 0, (x))
#define FREE(x) HeapFree(GetProcessHeap(), 0, (x))
#pragma comment(lib, "Iphlpapi.lib")
//Gavin: Necessary for M_PI to function properly
#define _USE_MATH_DEFINES // for C
#include <math.h>
#else
#include <ifaddrs.h>
#include <arpa/inet.h>
#include <cmath>
#include <utility>
#endif

#include "spdlog/spdlog.h"

using namespace aeye;

#ifdef _MSC_VER
std::string Sensor::getLocalIP(std::string& netInterface) {
  std::string ipaddr = "";
  PIP_ADAPTER_INFO pAdapterInfo;
  PIP_ADAPTER_INFO pAdapter = NULL;
  DWORD dwRetVal = 0;
  UINT i;

  struct tm newtime;
  char buffer[32];
  errno_t error;

  ULONG ulOutBufLen = sizeof(IP_ADAPTER_INFO);
  pAdapterInfo = (IP_ADAPTER_INFO*)MALLOC(sizeof(IP_ADAPTER_INFO));
  if (pAdapterInfo == NULL) {
      std::cout << "Error allocating memory needed to call GetAdaptersinfo" << std::endl;
      exit(1);
  }

  if (GetAdaptersInfo(pAdapterInfo, &ulOutBufLen) == ERROR_BUFFER_OVERFLOW) {
      FREE(pAdapterInfo);
      pAdapterInfo = (IP_ADAPTER_INFO*)MALLOC(ulOutBufLen);
      if (pAdapterInfo == NULL) {
          std::cout << "Error allocating memory needed to call GetAdaptersinfo" << std::endl;
          exit(1);
      }
  }

  if ((dwRetVal = GetAdaptersInfo(pAdapterInfo, &ulOutBufLen)) == NO_ERROR) {
      pAdapter = pAdapterInfo;
      while (pAdapter) {
          if ((std::string)pAdapter->Description == netInterface) {
              std::cout << "Found network interface:" << pAdapter->AdapterName << ", with IP:" << pAdapter->IpAddressList.IpAddress.String << std::endl;
              ipaddr = pAdapter->IpAddressList.IpAddress.String;
              break;
          }
          pAdapter = pAdapter->Next;
      }
  }
  else {
    printf("GetAdaptersInfo failed with error: %d\n", dwRetVal);
  }
  if (pAdapterInfo)
    FREE(pAdapterInfo);

  if (ipaddr.empty()) {
    std::stringstream ss;
    ss << "Network interface '" << netInterface << "' not available.";
    std::cout << ss.str() << " Assigning IP:127.0.0.1" << std::endl;
    ipaddr = "127.0.0.1";
    // Don't throw exception here! Otherwise we will not be able to edit this sensor in AEyeVis.
    //throw std::invalid_argument(ss.str());
  }

  return ipaddr;
}
#else
std::string Sensor::getLocalIP(std::string& netInterface) {
  std::string IPstr;
  struct ifaddrs* ifAddrStruct = nullptr;
  struct ifaddrs* ifa          = nullptr;
  getifaddrs(&ifAddrStruct);

  for (ifa = ifAddrStruct; ifa != nullptr; ifa = ifa->ifa_next) {
    if (ifa->ifa_addr == nullptr) {
      continue;
    }
    // Check that interface is IPv4
    if (ifa->ifa_addr->sa_family == AF_INET) {
      void* tmpAddrPtr  = &(reinterpret_cast<struct sockaddr_in*>(ifa->ifa_addr))->sin_addr;
      char addressBuffer[INET_ADDRSTRLEN];
      inet_ntop(AF_INET, tmpAddrPtr, addressBuffer, INET_ADDRSTRLEN);
      std::string tmpIPstr = addressBuffer;
      //std::cout << "local host:" << ifa->ifa_name << " : " << addressBuffer << std::endl;
      if (netInterface.empty() && !tmpIPstr.empty() && tmpIPstr != "127.0.0.1") {
        // Just in case if caller don't know interface name and provided empty string as parameter and we found valid one.
        std::cout << "Found network interface:" << ifa->ifa_name << ", with IP:" << tmpIPstr << std::endl;
        IPstr = tmpIPstr;
        netInterface = ifa->ifa_name;
        break;
      }
      else if (netInterface == ifa->ifa_name) {
        IPstr = tmpIPstr;
      }
    }
  }
  if (ifAddrStruct != nullptr) {
    freeifaddrs(ifAddrStruct);
  }
  if (IPstr.empty()) {
    std::stringstream ss;
    ss << "Network interface '" << netInterface << "' not available.";
    std::cout << ss.str() << " Assigning IP:127.0.0.1" << std::endl;
    IPstr = "127.0.0.1";
    // Don't throw exception here! Otherwise we will not be able to edit this sensor in AEyeVis.
    //throw std::invalid_argument(ss.str());
  }
  return IPstr;
}
#endif

// TODO deprecate this ctor
Sensor::Sensor(const std::string& netInterface, const std::string& ipAddr, const std::string& sensorName,
               uint16_t cmdPort, uint16_t clientPointsPort, uint16_t clientImagePort, uint16_t serverPointsPort,
               uint16_t serverImgPort, uint16_t clientDiagPort, const bool udpBroadcastMode)
               : Sensor(SensorConfig{clientDiagPort, toEnum<UdpBroadcastMode>(udpBroadcastMode)},
                        DataPortConfig{clientPointsPort}, netInterface, ipAddr, sensorName) {
  (void) cmdPort; (void) serverPointsPort; (void) serverImgPort; (void) clientImagePort; (void) cmdPort;
}

Sensor::Sensor(SensorConfig sensor_config, DataPortConfig data_ports,
               std::string net_interface, std::string sensor_ip, std::string sensor_name)
: clientInterface(std::move(net_interface)), sensorIp(std::move(sensor_ip)), name(std::move(sensor_name)),
  commandPort(DEFAULT_COMMAND_PORT), rangePort(data_ports.points),
  diagnosticPort(sensor_config.diagnostic_port), serverRangePort(DEFAULT_SERVER_POINTS_PORT),
  sensingOn(false), pcapModeOn(false), laserPower(),
  laserZone({0, 0, 0, 0, 0, 0, 0, 0}), laserZoneHorFOV(), laserZoneVerFOV(),
  pointReturnMask(PointReturnFlags::SPHERICAL_COORDINATES | PointReturnFlags::INTENSITY), passiveModeMask(PassiveModeFlags::DISABLED),
  pose(Pose{}) {
    if(sensor_ip == "127.0.0.1") {
        serverRangePort = static_cast<uint16_t>(DEFAULT_SERVER_POINTS_PORT);
    }

  spdlog::trace("Constructing Sensor with IP {} on {} named {}.", sensorIp, clientInterface, name);

  clientIp        = getLocalIP(clientInterface);
  spdlog::trace("Selected local host IP: {}, for sensor: {} with IP: {}.", clientIp, name, sensorIp);

  sensorReader = std::make_unique<SensorReader>(sensorIp, rangePort, diagnosticPort, DEFAULT_CLIENT_HAWKEYE_PORT,
                                                name, static_cast<bool>(sensor_config.broadcast_mode));
  sensorCommander = std::make_unique<SensorCommander>(sensorIp, commandPort);
}

Sensor::~Sensor() {
  try { stop(); } catch (std::exception& e) {
    spdlog::warn("stop() raised exception: {}.", e.what());
  }
}

Sensor::Quadrilateral& Sensor::Quadrilateral::operator =(const aeye::Quadrilateral& other) {
  topLeftX  = other.topLeftX;
  topLeftY  = other.topLeftY;
  topRightX = other.topRightX;
  topRightY = other.topRightY;
  btmLeftX  = other.btmLeftX;
  btmLeftY  = other.btmLeftY;
  btmRightX = other.btmRightX;
  btmRightY = other.btmRightY;
  return *this;
}

  void Sensor::setOnDiagnosticDataCallback(const std::function<void(DiagnosticData&&)>& processDiagnosticCallback) {
    sensorReader->setDiagnosticCallback(processDiagnosticCallback);
  }

  void Sensor::setOnPointCloudFrameCallback(const std::function<void(PointCloudFrame&&)>& processPointCloudCallback) {
    sensorReader->setCloudCallback(processPointCloudCallback);
  }

  void Sensor::connect() {
    if (pcapModeOn) return;

    sensorCommander->connectToSensor();
    sendPointReturnMask(aeye::PointReturnFlags::SPHERICAL_COORDINATES | aeye::PointReturnFlags::INTENSITY);
    sendIntensityType(0); // Defaulting to REFLECTIVITY_V1 on every startup.
    sensorModelType = sensorCommander->cmdGetSensorModel();
    collectSensorState();  //Update cached values to match the sensor's state
  }

  void Sensor::start() {
    sensingOn = true;

    sensorReader->start();
    if (!pcapModeOn) {
      sensorCommander->cmdStart(clientIp, rangePort);
    }
  }

void Sensor::startDiagnostic() {
  sensorReader->startDiagnostic();
}

void Sensor::disconnect() {
  try { stop(); } catch (std::exception& e) {
    spdlog::warn("stop() raised exception: {}.", e.what());
  }
  try { sensorCommander->disconnectFromSensor(); } catch (std::exception& e) {
    spdlog::warn("disconnectFromSensor() raised exception: {}.", e.what());
  }
  sensorModelType = ModelType::UNKNOWN;
}

  void Sensor::stop() {
    if (!sensingOn) return;

    sensingOn = false;
    if (!pcapModeOn) {
      try {
        sensorCommander->cmdStop();
      } catch (const std::exception &e) {
        spdlog::warn("stop() raised exception: {}.", e.what());
      }
    }
    sensorReader->stop();
  }

  void Sensor::reboot() {
    sensorCommander->cmdReboot();
  }

  bool Sensor::isConnected() const {
      if (!pcapModeOn) { return sensorCommander->isConnected(); }
      else return true;
  }

  int8_t Sensor::getCurrentShotlistIndex() {
    int8_t currentShotlistIndex = -1;
    std::stringstream ss;
    try {
      if (!pcapModeOn) {
        ///Passive Sensing State
        auto getPassiveSensingStateBodyReturn = sensorCommander->getPassiveSensingState();
        passiveModeMask = getPassiveSensingStateBodyReturn.passiveSensingMode;

        // Check if we're in the shotlist mode
        if (passiveModeMask & PassiveModeFlags::SHOTLIST_FROM_FILE)
          currentShotlistIndex = getPassiveSensingStateBodyReturn.shotlistIdx;
        else
          currentShotlistIndex = -1;
      }

    }
    catch (std::exception& e) {
      ss << "Error in sensorCommander->getPassiveSensingState(): " << e.what() << std::endl;
    }
    return currentShotlistIndex;
  }

  bool Sensor::collectSensorState() {
      if (pcapModeOn) { return true; }

      bool isSuccess = true;
      std::stringstream ss;

      try {
          ///Passive Sensing State
          aeye::PacketGetPassiveSensingStateBody getPassiveSensingStateBodyReturn = sensorCommander->getPassiveSensingState();

          passiveModeMask = getPassiveSensingStateBodyReturn.passiveSensingMode;

          scanPatternFileNames.clear();
          size_t i = 0;
          while (true) {
            std::string shotlistName = getShotlistName(i);
            if (shotlistName.empty()) { break; }
            scanPatternFileNames.emplace_back(shotlistName);
            i++;
          }

      }
      catch (std::exception& e) {
          isSuccess = false;
          ss << "Error in sensorCommander->getPassiveSensingState(): " << e.what() << std::endl;
      }

      try {
          ///Pose
          aeye::PacketGetPoseBody getPoseBody = sensorCommander->getPoseFromSensor();
          pose = Pose{getPoseBody.x, getPoseBody.y, getPoseBody.z,
                      getPoseBody.yaw * (180/M_PI), getPoseBody.pitch * (180/M_PI), getPoseBody.roll * (180/M_PI)};
      }
      catch (std::exception& e) {
          isSuccess = false;
          ss << "Error in sensorCommander->getPoseFromSensor(): " << e.what() << std::endl;
      }

      try {
          ///Laser
          aeye::PacketGetLaserStateBodyReturnV2 getLaserBody = sensorCommander->getLaserState();
          laserPower          = getLaserBody.laserPower;
          laserZone.btmLeftX  = getLaserBody.laserZone.btmLeftX;
          laserZone.btmLeftY  = getLaserBody.laserZone.btmLeftY;
          laserZone.topLeftX  = getLaserBody.laserZone.topLeftX;
          laserZone.topLeftY  = getLaserBody.laserZone.topLeftY;
          laserZone.btmRightX = getLaserBody.laserZone.btmRightX;
          laserZone.btmRightY = getLaserBody.laserZone.btmRightY;
          laserZone.topRightX = getLaserBody.laserZone.topRightX;
          laserZone.topRightY = getLaserBody.laserZone.topRightY;
          laserZoneHorFOV     = getLaserBody.laserHFovDegree;
          laserZoneVerFOV     = getLaserBody.laserVFovDegree;


      }
      catch (std::exception& e) {
          isSuccess = false;
          ss << "Error in sensorCommander->getLaserState(): " << e.what() << std::endl;
      }

      try {
          // Ranging
          aeye::PacketGetRangingStateBodyReturn rangingStateBody = sensorCommander->getRangingState();
          pointReturnMask = rangingStateBody.pointReturnMask;
      }
      catch (std::exception& e) {
          isSuccess = false;
          ss << "Error in sensorCommander->getRangeDelayState(): " << e.what() << std::endl;
      }

      try {
          PacketBuildInfoBody body;
          std::tie(buildInfo, body) = sensorCommander->cmdGetBuildInfo();

      }
      catch (std::exception& e) {
        isSuccess = false;
        ss << "Sensor : getBuildInfo : raised an exception " << e.what() << std::endl;
      }

      try {
        PacketBuildInfoExtraBody body = sensorCommander->getBuildInfoExtra();
        scanPatternFlags = body.scanPatternFlags;
        scanBlockId = body.scanblockId;
        macAddress = std::string{body.macAddress};
      } catch (std::exception& e) {
        isSuccess = false;
        ss << "Sensor : getBuildInfoExtra : raised an exception " << e.what() << std::endl;
      }

      if (!isSuccess) {
        spdlog::error(ss.str());
      }
      return isSuccess;
  }

  const std::string& Sensor::getBuildInfo() const {
    return buildInfo;
  }

  ModelType Sensor::getSensorModel() const {
      return sensorModelType;   // return already retrieved value
  }

  void Sensor::sendTime() {
      try { sensorCommander->setTime(); } catch (std::exception& e) {
          spdlog::warn("sensorCommander->setTime() raised exception: {}.", e.what());
      }
  }

  void Sensor::sendNetworkConfig(const std::string& server_ip, const std::string& netmask,
                                 const std::string& gateway) {
      try { sensorCommander->setNetworkConfig(server_ip, netmask, gateway, DEFAULT_COMMAND_PORT); } catch (std::exception& e) {
          spdlog::warn("sensorCommander->setNetworkConfig() raised exception: {}.", e.what());
      }
  }

  void Sensor::getNetworkConfig(std::string& server_ip, std::string& netmask,
                                std::string& gateway) {
    try {
      uint16_t cmdport;
      sensorCommander->getNetworkConfig(server_ip, netmask, gateway, cmdport);
    } catch (std::exception& e) {
      spdlog::warn("sensorCommander->setNetworkConfig() raised exception: {}.", e.what());
    }
  }

  void Sensor::sendNtpServer(bool enabled_ntp, const std::string& ntp_ip) {
    try { sensorCommander->setNtpServer(enabled_ntp, ntp_ip); } catch (std::exception& e) {
      spdlog::warn("sensorCommander->setNtpServer() raised exception: {}.", e.what());
    }
  }

  void Sensor::sendPtpServer(bool enabled_ptp) {
    try { sensorCommander->setPtpServer(enabled_ptp); } catch (std::exception& e) {
      spdlog::warn("sensorCommander->setPtpServer() raised exception: {}.", e.what());
    }
  }

  void Sensor::sendGptpServer(bool enabled_gptp) {
    try { sensorCommander->setGptpServer(enabled_gptp); } catch (std::exception& e) {
      spdlog::warn("sensorCommander->setGptpServer() raised exception: {}.", e.what());
    }
  }

   void Sensor::sendPointReturnMask(uint16_t pointReturnMask) {
    try {
      sensorCommander->setPointReturnMask(pointReturnMask);
      this->pointReturnMask = pointReturnMask;
    } catch (std::exception& e) {
      spdlog::warn("sensorCommander->setPointReturnMask() raised exception: {}.", e.what());
    }
  }

  void Sensor::sendIntensityType(uint8_t intensityType) {
    try {
      sensorCommander->setIntensityType(intensityType);
    } catch (std::exception& e) {
      spdlog::warn("sensorCommander->setIntensityType() raised exception: {}.", e.what());
    }
  }

  // NOTE: not yet deprecated: currently only used to enable scan pattern
  void Sensor::sendPassiveMode(uint8_t passiveModeMask) {
      try {
          sensorCommander->setPassiveSensingMode(passiveModeMask);
          this->passiveModeMask = passiveModeMask;
      } catch (std::exception& e) {
          spdlog::warn("sensorCommander->setPassiveSensingMode() raised exception: {}.", e.what());
          throw;
      }
  }


void Sensor::sendShotlistFileNumber(int value) {
  try {
    sensorCommander->setPassiveModeParameter(aeye::PassiveModeParam::SHOTLIST_FILE_NUMBER, value);
  } catch (std::exception& e) {
    spdlog::warn("sensorCommander->sendShotlistFileNumber(aeye::PassiveModeParam::SHOTLIST_FILE_NUMBER, value) raised exception: {}.", e.what());
  }
}

void Sensor::sendNumMultiEchoes(uint8_t numEchoes)
{
  try
  {
    sensorCommander->sendNumMultiEchoes(numEchoes);
    this->numEchoes = numEchoes;
  }
  catch (std::exception& e)
  {
    spdlog::warn("sensorCommander->sendNumMultiEchoes() raised exception: {}.", e.what());
  }
}

uint8_t Sensor::getNumMultiEchoes() {
  try
  {
    if (!pcapModeOn) {
      PacketGetNumMultiEchoes echoes = sensorCommander->getNumMultiEchoes();
      this->numEchoes = echoes.numEchoes;
    }
    else {
      this->numEchoes = 1;
    }
    return this->numEchoes;
  }
  catch (std::exception& e)
  {
    spdlog::warn("sensorCommander->getNumMultiEchoes() raised exception: {}.", e.what());
    throw;
  }
}

void Sensor::setModeMultiEchoes(MultiEchoMode mode) {
  try {
    sensorCommander->setModeMultiEchoes(mode);
    this->modeMultiEchoes = mode;
  }
  catch (std::exception& e)
  {
    spdlog::warn("sensorCommander->setModeMultiEchoes() raised exception: {}.", e.what());
    throw;
  }
}

MultiEchoMode Sensor::getModeMultiEchoes() {
  try {
    modeMultiEchoes = sensorCommander->getModeMultiEchoes().mode;
    return modeMultiEchoes;
  }
  catch (std::exception& e) {
    spdlog::warn("sensorCommander->getModeMultiEchoes() raised exception: {}.", e.what());
    throw;
  }
}

void Sensor::setSkirtThreshNearHigh(uint16_t value) {
  try {
    sensorCommander->setSkirtThreshNearHigh(value);
  } catch (std::exception &e) {
    spdlog::warn("sensorCommander->setSkirtThreshNearHigh() raised exception: {}.", e.what());
    throw;
  }
}

void Sensor::setSkirtThreshNearLow(uint16_t value) {
  try {
    sensorCommander->setSkirtThreshNearLow(value);
  } catch (std::exception &e) {
    spdlog::warn("sensorCommander->setSkirtThreshNearLow() raised exception: {}.", e.what());
    throw;
  }
}

void Sensor::setSkirtThreshFarHigh(uint16_t value) {
  try {
    sensorCommander->setSkirtThreshFarHigh(value);
  } catch (std::exception &e) {
    spdlog::warn("sensorCommander->setSkirtThreshFarHigh() raised exception: {}.", e.what());
    throw;
  }
}

void Sensor::setSkirtThreshFarLow(uint16_t value) {
  try {
    sensorCommander->setSkirtThreshFarLow(value);
  } catch (std::exception &e) {
    spdlog::warn("sensorCommander->setSkirtThreshFarLow() raised exception: {}.", e.what());
    throw;
  }
}

uint16_t Sensor::getSkirtThreshNearHigh() {
  uint16_t value = 0;
  try {
    value = sensorCommander->getSkirtThreshNearHigh();
  } catch (std::exception& e) {
    spdlog::warn("sensorCommander->getSkirtThreshNearHigh() raised exception: {}.", e.what());
  }
  return value;
}

uint16_t Sensor::getSkirtThreshNearLow() {
  uint16_t value = 0;
  try {
    value = sensorCommander->getSkirtThreshNearLow();
  } catch (std::exception& e) {
    spdlog::warn("sensorCommander->getSkirtThreshNearLow() raised exception: {}.", e.what());
  }
  return value;
}

uint16_t Sensor::getSkirtThreshFarHigh() {
  uint16_t value = 0;
  try {
    value = sensorCommander->getSkirtThreshFarHigh();
  } catch (std::exception& e) {
    spdlog::warn("sensorCommander->getSkirtThreshFarHigh() raised exception: {}.", e.what());
  }
  return value;
}

uint16_t Sensor::getSkirtThreshFarLow() {
  uint16_t value = 0;
  try {
    value = sensorCommander->getSkirtThreshFarLow();
  } catch (std::exception& e) {
    spdlog::warn("sensorCommander->getSkirtThreshFarLow() raised exception: {}.", e.what());
  }
  return value;
}

std::string Sensor::getShotlistName(uint16_t i) {
  std::string value;
  try {
    value = sensorCommander->getShotlistName(i);
  } catch (std::exception& e) {
    spdlog::warn("sensorCommander->getShotlistName() raised exception: {}.", e.what());
  }
  return value;
}

TimeServerStatus Sensor::getNtpStatus() {
  PacketGetNtpStatus packet;
  try {
    packet = sensorCommander->getNtpStatus();
    this->NTPstatus = packet.NTPstatus;
    this->timeCorrectness = packet.timeCorrectness;
    this->timeConstant = packet.timeConstant;
  } catch (std::exception& e) {
    spdlog::warn("sensorCommander->getNtpStatus() raised exception: {}.", e.what());
    this->NTPstatus = TimeServerStatus::INDETERMINATE;
    this->timeCorrectness = 0;
    this->timeConstant = 0;
  }
  return this->NTPstatus;
}

TimeServerStatus Sensor::getPtpStatus() {
  PacketGetPtpStatus packet;
  try {
    packet = sensorCommander->getPtpStatus();
    this->PTPstatus = packet.PTPstatus;
    this->PTPtimeStamp = packet.timeStamp;
    this->PTPstate = packet.state;
    this->PTPclockId = packet.clockId;
    this->PTPoneWayDelay = packet.oneWayDelay;
    this->PTPoffsetFromMaster = packet.offsetFromMaster;
    this->PTPslaveToMaster = packet.slaveToMaster;
    this->PTPmasterToSlave = packet.masterToSlave;
    this->PTPobservedDrift = packet.observedDrift;
    this->PTPlastPacketReceived = packet.lastPacketReceived;
  } catch (std::exception& e) {
    spdlog::warn("sensorCommander->getPtpStatus() raised exception: {}.", e.what());
    this->PTPstatus = TimeServerStatus::INDETERMINATE;
    this->PTPtimeStamp = "";
    this->PTPstate = "";
    this->PTPclockId ="" ;
    this->PTPoneWayDelay = 0;
    this->PTPoffsetFromMaster = 0;
    this->PTPslaveToMaster = 0;
    this->PTPmasterToSlave = 0;
    this->PTPobservedDrift = 0;
    this->PTPlastPacketReceived = ' ';
  }
  return this->PTPstatus;
}

TimeServerStatus Sensor::getGptpStatus() {
  try {
    PacketGetGptpStatus packet = sensorCommander->getGptpStatus();
    gPtpServerStatusParams.GPTPstatus = packet.gPTPstatus;
    gPtpServerStatusParams.stepsRemoved = packet.stepsRemoved;
    gPtpServerStatusParams.offsetFromMaster = packet.offsetFromMaster;
    gPtpServerStatusParams.meanPathDelay = packet.meanPathDelay;
    gPtpServerStatusParams.masterOffset = packet.masterOffset;
    gPtpServerStatusParams.ingressTime = packet.ingress_time;
    gPtpServerStatusParams.cumulativeScaledRateOffset = packet.cumulativeScaledRateOffset;
    gPtpServerStatusParams.scaledLastGmPhaserChange = packet.scaledLastGmPhaserChange;
    gPtpServerStatusParams.gmPresent = packet.gmPresent;
    gPtpServerStatusParams.gmIdentity = packet.gmIdentity;
    gPtpServerStatusParams.portIdentity = packet.portIdentity;
    gPtpServerStatusParams.portState = packet.portState;
    gPtpServerStatusParams.logMinDelayRequestInterval = packet.logMinDelayReqInterval;
    gPtpServerStatusParams.peerMeanPathDelay = packet.peerMeanPathDelay;
    gPtpServerStatusParams.logAnnounceInterval = packet.logAnnounceInterval;
    gPtpServerStatusParams.announceReceiptTimeout = packet.announceReceiptTimeout;
    gPtpServerStatusParams.logSyncInterval = packet.logSyncInterval;
    gPtpServerStatusParams.delayMechanism = packet.delayMechanism;
    gPtpServerStatusParams.logMindPdelayReqInterval = packet.logMinPdelayReqInterval;
    gPtpServerStatusParams.versionNumber = packet.versionNumber;
  } catch (std::exception& e) {
    spdlog::warn("sensorCommander->getGptpStatus() raised exception: {}.", e.what());
    gPtpServerStatusParams = {};
    gPtpServerStatusParams.GPTPstatus = TimeServerStatus::INDETERMINATE;
  }
  return gPtpServerStatusParams.GPTPstatus;
}

float Sensor::getTimeCorrectness() const {
  return timeCorrectness;
}
float Sensor::getTimeConstant() const {
  return timeConstant;
}

std::string Sensor::getPTPTimestamp() const { return PTPtimeStamp; }
std::string Sensor::getPTPState() const { return PTPstate; }
std::string Sensor::getPTPClockID() const { return PTPclockId; }
float Sensor::getPTPOneWayDelay() const { return PTPoneWayDelay; }
float Sensor::getPTPOffsetFromMaster() const { return PTPoffsetFromMaster; }
float Sensor::getPTPSlaveToMaster() const { return PTPslaveToMaster; }
float Sensor::getPTPMasterToSlave() const { return PTPmasterToSlave; }
float Sensor::getPTPObservedDrift() const { return PTPobservedDrift; }
std::string Sensor::getPTPLastPacketReceived() const { return PTPlastPacketReceived; }

GptpServerStatus Sensor::getGptpServerStatusParams() const {return gPtpServerStatusParams; }

  // Getters
  const std::string& Sensor::getInterface() const { return clientInterface; }
  const std::string& Sensor::getSensorIp()  const { return sensorIp;  }
  const std::string& Sensor::getName()      const { return name;      }
  const std::string& Sensor::getClientIp()  const { return clientIp;  }

  uint16_t Sensor::getCommandPort()     const { return commandPort;     }
  uint16_t Sensor::getRangePort()       const { return rangePort;       }
  uint16_t Sensor::getServerRangePort() const { return serverRangePort; }
  bool     Sensor::isSensing()          const { return sensingOn;       }
  bool     Sensor::isPcapMode()         const { return pcapModeOn;      }

  // NOTE: still needed to detect scan pattern from file usage - need to add additional command/variable
  uint8_t             Sensor::getPassiveModeMask()               const { return passiveModeMask;                     }

  uint16_t Sensor::getPointReturnMask() const { return pointReturnMask; }

  uint16_t              Sensor::getLaserPower()         const { return laserPower;                                 }
  Sensor::Quadrilateral Sensor::getLaserZone()          const { return laserZone;                                  }
  uint16_t              Sensor::getLaserZoneHorFOV()    const { return static_cast<uint16_t>(laserZoneHorFOV*100); }
  uint16_t              Sensor::getLaserZoneVerFOV()    const { return static_cast<uint16_t>(laserZoneVerFOV*100); }

  const Pose&         Sensor::getPose()               const { return pose;               }

  // Setters
  void Sensor::setName(const std::string& sensorName) { name             = sensorName; }
  void Sensor::setPcapMode(bool value) {
    pcapModeOn       = value;
    sensorReader->setPcapMode(value);
  }

  void Sensor::sendPose(double x, double y, double z, double yaw, double pitch, double roll) {
      try {
          // Internally convert radians to degrees before sending it to the sensor
          double yaw_rad = yaw  * M_PI / 180;
          double pitch_rad = pitch  * M_PI / 180;
          double roll_rad = roll  * M_PI / 180;
          sensorCommander->setPose(x, y, z, yaw_rad, pitch_rad, roll_rad);
          pose = Pose{x, y, z, yaw, pitch, roll};
      } catch (std::exception& e) {
          spdlog::warn("sensorCommander->setPose() raised exception: {}.", e.what());
      }
  }

  void Sensor::processPcapPacket(PcapPacket& packet) {
      if (packet.sourceIpAddress() != sensorIp) { return; }
      if (!(pcapModeOn && sensingOn)) { return; }

      if (packet.sourcePort() == rangePort) {
        sensorReader->processRangingPcapPacket(packet);
      }
      else if (packet.sourcePort() == diagnosticPort) {
        sensorReader->processDiagnosticPcapPacket(packet);
      }
  }

void Sensor::printState(std::ostream& out = std::cout) {
      out << "Client IP address is: "       << clientIp                                       << '\n';
      out << "Network interface is: "       << clientInterface                                << '\n';
      out << "Sensor IP address is: "       << sensorIp                                       << '\n';
      out << "Sensor name is: "             << name                                           << '\n';
      out << "Version of the sensor is : "  << buildInfo                                      << '\n';
      out << "Client command port is: "     << commandPort                                    << '\n';
      out << "Client point cloud port is: " << rangePort                                      << '\n';
      out << "Server point cloud port is: " << serverRangePort                                << '\n';
      out << "The sensor is "               << (sensingOn ? "" : "not ")   << "sensing"       << '\n';
      out << "PCap playback is "            << (pcapModeOn  ? "" : "not ") << "enabled"       << '\n';

      out << "---------------------------------------------------------------------------"    << '\n';

      out << "Laser settings"                                                                 << '\n';
      out << "The laser power is "               << laserPower                                << '\n';
      out << "Laser Zone : "                                                                  << '\n';
      out << "(" << laserZone.topLeftX << ", "   << laserZone.topLeftY<< "), "
          << "(" << laserZone.topRightX<< ", "   << laserZone.topRightY << ")"                << '\n';
      out << "(" << laserZone.btmLeftX << ", "   << laserZone.btmLeftY<< "), "
          << "(" << laserZone.btmRightX<< ", "   << laserZone.btmRightY << ")"                << '\n';
      out << "The LiDAR's horizontal FOV is: "   << laserZoneHorFOV << " degrees"             << '\n';
      out << "The LiDAR's vertical FOV is: "     << laserZoneVerFOV << " degrees"             << '\n';

      out << "---------------------------------------------------------------------------"    << '\n';

      out << "PointReturn mask is : " << pointReturnMask                                        << '\n';
      out << "Unused_0 flag       : " << ((pointReturnMask & aeye::UNUSED_0) > 0)               << '\n';
      out << "Spherical flag      : " << ((pointReturnMask & aeye::SPHERICAL_COORDINATES) > 0)  << '\n';
      out << "UNUSED_1 flag       : " << ((pointReturnMask & aeye::UNUSED_1) > 0)                  << '\n';
      out << "Intensity flag      : " << ((pointReturnMask & aeye::INTENSITY) > 0)              << '\n';
      out << "UNUSED_2 flag       : " << ((pointReturnMask & aeye::UNUSED_2) > 0)      << '\n';
      out << "Unused_3 flag       : " << ((pointReturnMask & aeye::UNUSED_3) > 0)               << '\n';
      out << "Unused_4 flag       : " << ((pointReturnMask & aeye::UNUSED_4) > 0)               << '\n';
      out << "UNUSED_5 flag       : " << ((pointReturnMask & aeye::UNUSED_5) > 0)                  << '\n';
      out << "UNUSED_6 flag       : " << ((pointReturnMask & aeye::UNUSED_6) > 0)              << '\n';
      out << "UNUSED_7 flag       : " << ((pointReturnMask & aeye::UNUSED_7) > 0)             << '\n';
      out << "UNUSED_8 flag       : " << ((pointReturnMask & aeye::UNUSED_8) > 0)          << '\n';

      out << "---------------------------------------------------------------------------"    << '\n';

      out << "Sensor pose: " << '\n' <<
      "x: "   << pose.x   << '\n' << "y: "     << pose.y     << '\n' <<  "z: "   << pose.z    << '\n' <<
      "yaw: " << pose.yaw << '\n' << "pitch: " << pose.pitch << '\n' << "roll: " << pose.roll << '\n';
}
const std::vector<std::string>& Sensor::getScanPatternFileNames() {
  return scanPatternFileNames;
}

void Sensor::resetPlaybackState() {
  sensorReader->resetSocketReaders();
}

void Sensor::SetTCPKeepAlive(bool enable) {
  sensorCommander->SetTCPKeepAlive(enable);
}

bool Sensor::GetTCPKeepAlive() { return sensorCommander->GetTCPKeepAlive(); }
